/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Robo;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.SmartDashboard;
import edu.wpi.first.wpilibj.Solenoid;

/**
 *
 * @author paul
 */
public class RoboDrive extends RoboTask
{
    // transmission
    private Compressor compressor= new Compressor(4,1,4,1);
    private Solenoid solenoid1 = new Solenoid(7, 1);
    private Solenoid solenoid2 = new Solenoid(7, 2);
    private boolean automatic = false;
    private boolean highGear = false;
    public double averageSpeed = 0;
    public double upShiftSpeed = 45.0;
    public double downShiftSpeed = 44.0;

    // encoders
    public Encoder leftEncoder = new Encoder(4, 2, 4, 3);
    public Encoder rightEncoder = new Encoder(4, 4, 4, 5);
    public double leftDistance;
    public double rightDistance;
    public double rotationDistance;

    // encoder scales
    public double inchesPerTick = 0.072;
    public double lastLeft = 0;
    public double lastRight = 0;
    public double lastRotation = 0;
    public double lastTime = 0;
    public double lastDistanceTime = 0; // distance and speed m,easurement time base
    public double leftSpeed = 0;
    public double rightSpeed = 0;
    public double motionSpeed;
    public double rotationSpeed;
    public double motionThrottle = 0;
    public double rotationThrottle = 0;


    // drive control modes
    protected static final int TankMode = 0;
    protected static final int SmartMode = 1;
    public int controlMode = TankMode;
    protected boolean reversed = false;
    protected boolean autoTurnRight = false; // true = rotationDistance increasing
    protected boolean autoTurnActive = false;
    protected boolean flipAfterAutoTurn = false;
    protected double autoTurnTarget;
    
    // drive control constants
    private static final double turn90 = 30;
    private static final double turn180 = 60;
    
    // smart drive controllers
    double targetMotion = 0;
    double targetRotation = 0;
    double steering = 0;    // desired turn speed +/-1 (+=clockwise)
    double throttle = 0;    // desired throttle +/-1 (+=current "forward")

    // drive train components
    final int frontLeft = 2;
    final int frontRight = 4;
    final int rearLeft = 1;
    final int rearRight = 3;
    final int port1 = 1;
    final int port2 = 2;
    private RobotDrive teamRobot = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);

    // misc state variables
    private double lastDisplayTime = 0;
    // robot drive base object

    RoboDrive(){
        //teamRobot.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
        //teamRobot.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
        // set the MotorSafety expiration timer
        teamRobot.setExpiration(15);
        leftEncoder.setDistancePerPulse(inchesPerTick);
        rightEncoder.setDistancePerPulse(inchesPerTick);
    }

    public void start(){
        super.start();
        
        // init encoders
        leftEncoder.start();
        rightEncoder.start();
        leftEncoder.setDistancePerPulse(inchesPerTick);
        rightEncoder.setDistancePerPulse(inchesPerTick);
        
        // init controllers
        /*
        motionPID.setPID(0.01, 0.0, 0.0);
        motionPID.setInputRange(-180, +180);    // speed range
        motionPID.setOutputRange(-1.0, +1.0);    // throttle range
        motionPID.setTolerance(2.0);
        motionPID.setSetpoint(0.0);
        motionPID.enable();
        
        rotationPID.setPID(0.01, 0.0, 0.0);
        rotationPID.setInputRange(-120, +120);    // rotation speed range
        rotationPID.setOutputRange(-2.0, +2.0);    // throttle delta range
        rotationPID.setTolerance(2.0);
        rotationPID.setSetpoint(0.0);
        rotationPID.enable();

         */
        
        // init auto drive
        autoTurnActive = false;
        reversed = false;
        flipAfterAutoTurn = false;
        targetMotion = 0;
        targetRotation = 0;
        
        teamRobot.tankDrive(0, 0);

        // init transmission
        highGear = false;
        automatic = false;
        compressor.start();
        solenoid1.set(false);
        solenoid2.set(true);

        lastDisplayTime = 0;
        lastTime = 0;
    }

    public void handle(){
        super.handle();

        leftDistance = leftEncoder.getDistance();
        rightDistance = rightEncoder.getDistance();

        // adjust speed readings only if at least some time has expired
        if(taskTime > (lastDistanceTime + 0.1)){
            // set PID speed inputs
            leftSpeed = ((leftDistance - lastLeft) / (taskTime - lastDistanceTime));
            rightSpeed = ((rightDistance - lastRight) / (taskTime - lastDistanceTime));
            motionSpeed = (rightSpeed + leftSpeed) / 2;
            averageSpeed = (Math.abs(rightSpeed) + Math.abs(leftSpeed)) / 2;

            rotationDistance = leftDistance - rightDistance;
            rotationSpeed = (rotationDistance - lastRotation) / (taskTime - lastDistanceTime);

            lastLeft = leftDistance;
            lastRight = rightDistance;
            lastRotation = rotationDistance;
            lastDistanceTime = taskTime;
        }
        // handle transmission
        if(robot.driverStation.leftDriveStick.getRawButton(7)) automatic = true;
        if(robot.driverStation.leftDriveStick.getRawButton(6)) automatic = false;

        if(highGear) {
            if((automatic && averageSpeed < downShiftSpeed) ||
                    (!automatic && robot.driverStation.leftDriveStick.getRawButton(2))){
                solenoid1.set(false);
                solenoid2.set(true);
                highGear = false;
            }
        }
        else {
            if((automatic && averageSpeed > upShiftSpeed) ||
                    (!automatic && robot.driverStation.leftDriveStick.getRawButton(3))){
                solenoid1.set(true);
                solenoid2.set(false);
                highGear = true;
            }
        }
        
        // handle driving
       
        // drive mode switching
        if(robot.driverStation.leftDriveStick.getRawButton(8)) controlMode = TankMode;
        if(robot.driverStation.leftDriveStick.getRawButton(9)) controlMode = SmartMode;
        // odometer reset
        if(robot.driverStation.leftDriveStick.getRawButton(10)) {
            leftEncoder.reset();
            rightEncoder.reset();
        }
        if(controlMode == SmartMode){
            // auto-turns
            if(robot.driverStation.rightDriveStick.getRawButton(4)) {
                // left-90
                autoTurnTarget = rotationDistance - turn90;
                autoTurnRight = false;
                autoTurnActive = true;
                flipAfterAutoTurn = false;
            }
            if(robot.driverStation.rightDriveStick.getRawButton(5)) {
                // right-90
                autoTurnTarget = rotationDistance + turn90;
                autoTurnRight = true;
                autoTurnActive = true;
                flipAfterAutoTurn = false;
            }
            if(robot.driverStation.rightDriveStick.getRawButton(2)) {
                // reverse on
                reversed = true;
                flipAfterAutoTurn = false;
            }
            if(robot.driverStation.rightDriveStick.getRawButton(1)) {
                // reverse off
                reversed = false;
                flipAfterAutoTurn = false;
                autoTurnActive = false;
            }
            if(robot.driverStation.rightDriveStick.getRawButton(3)) {
                // flip
                autoTurnTarget = rotationDistance + turn180;
                autoTurnRight = true;
                flipAfterAutoTurn = true;
                autoTurnActive = true;
            }
        }        

        // driving
        double leftY = robot.driverStation.leftDriveStick.getY();
        double rightY = robot.driverStation.rightDriveStick.getY();
        double leftX = robot.driverStation.leftDriveStick.getX();
        double rightX = robot.driverStation.rightDriveStick.getX();

        // setpoints for PIDs (set, may be ignored if in manual mode)
        targetMotion = rightY * 180;
        targetRotation = rightX * 120;
        steering = rightX;
        throttle = leftY;

        switch(controlMode){
            case TankMode:
               teamRobot.tankDrive(-rightY, -leftY);
               break;
            case SmartMode:
                {
                    double lt, rt;

                    double dm = targetMotion - motionSpeed;
                    double dr = targetRotation - rotationSpeed;
                    
                    // make any adjustments (or override PID altogether)
                    lt = rt = targetMotion / 180;
                    
                    lt += targetRotation / 120;
                    rt -= targetRotation / 120;
                    
                    // apply any auto-turns (overrides other controls!
                    if(autoTurnActive){
                        if(autoTurnRight){
                            if(rotationDistance < autoTurnTarget){
                                // keep turning right
                                lt = 1;
                                rt = -1;
                            }
                            else {
                                // done
                                autoTurnActive = false;
                                if(flipAfterAutoTurn) reversed = !reversed;
                                flipAfterAutoTurn = false;
                            }
                        }
                        else {
                            if(rotationDistance > autoTurnTarget){
                                // keep turning left
                                lt = -1;
                                rt = 1;
                            }
                            else {
                                // done
                                autoTurnActive = false;
                                if(flipAfterAutoTurn) reversed = !reversed;
                                flipAfterAutoTurn = false;
                            }
                        }
                    }
            
                    // apply throttle limits
                    if(lt > 1.0) lt = 1.0;
                    if(lt < -1.0) lt = -1.0;
                    if(rt > 1.0) rt = 1.0;
                    if(rt < -1.0) rt = -1.0;
                    
                    if(reversed){
                        double t = lt;
                        lt = -rt;
                        rt = -t;
                    }
            
                    // apply throttles
                    teamRobot.tankDrive(-lt, -rt);
                }
                break;
        }
        
        // save last data for the next iteration
        lastTime = taskTime;
        
        // display some data (periodically, do not flood the dashboard)
        if(taskTime > (lastDisplayTime + 0.25)){
            lastDisplayTime = taskTime;
            SmartDashboard.log(taskTime, "Time");
            SmartDashboard.log((automatic == true)?"AUTOMATIC":"MANUAL", "Transmission mode");
            SmartDashboard.log((controlMode == TankMode)?"TANK":"AUTO", "Control mode");
            SmartDashboard.log(leftDistance, "Left distance");
            SmartDashboard.log(rightDistance, "Right distance");
            SmartDashboard.log(motionSpeed, "Motion speed");
            SmartDashboard.log(averageSpeed, "Abs. speed (trans)");
            SmartDashboard.log(rotationSpeed, "Rotation speed");

            SmartDashboard.log(targetMotion, "Target motion");
            SmartDashboard.log(targetRotation, "Target rotation");

           // SmartDashboard.log(motionThrottle.get(), "Motion throttle");
            //SmartDashboard.log(rotationThrottle.get(), "Rotation throttle");
        }
    }

    public void stop(){
        super.stop();
        compressor.stop();
        teamRobot.tankDrive(0, 0);
    }
}
